/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef ALGRORITHM_H
#define ALGRORITHM_H

#include "common_include.h"

inline bool triangulation(const std::vector<SE3> &poses, const std::vector<Vec3> points,
                          Vec3 &pt_world){
    MatXX A(2*poses.size(), 4);
    VecX b(2*poses.size());
    b.setZero();
    for(size_t i=0; i<pose.size();++i){
    Mat34 m=poses[i].matrix3x4();
    A.block<1,4>(2*i, 0)=points[i][0]*m.row(2)-m.row(0);
    A.block<1,4>(2*i+1, 0)=points[i][1]*m.row(2)-m.row(1);
    }
    
    auto svd=A.bdcSvd(Eigen::ComputeThinU|Eigen::ComputeThinV);
    pt_world=(svd.matrixV().col(3)/svd.matrixV()(3,3)).head<3>();
    
    if(svd.singularValues()[3]/svd.singularValues()[2]<1e-2){
        return true;
    }
    return false;
}

inline Vec2 toVec2(const cv::Points p) {return Vec2(p.x, p.y);}
    

#endif // ALGRORITHM_H
